Fast Marching Tree: a Fast Marching Sampling-Based Method for Optimal Motion Planning in Many Dimensions

In this paper we present a novel probabilistic sampling-based motion planning algorithm called the Fast Marching Tree algorithm (FMT∗). The algorithm is specifically aimed at solving complex motion planning problems in high-dimensional configuration spaces. This algorithm is proven to be asymptotically optimal and is shown to converge to an optimal solution faster than its state-of-the-art counterparts, chiefly PRM∗ and RRT∗ . An additional advantage of FMT∗ is that it builds and maintains paths in a tree structure (especially useful for planning under differential constraints). The FMT∗ algorithm essentially performs a “lazy” dynamic programming recursion on a set of probabilistically-drawn samples to grow a tree of paths, which moves steadily outward in cost-to-come space. As such, this algorithm combines features of both single-query algorithms (chiefly RRT) and multiple-query algorithms (chiefly PRM), and is conceptually related to the Fast Marching Method for the solution of eikonal equations. As a departure from previous analysis approaches that are based on the notion of almost sure convergence, the FMT∗ algorithm is analyzed under the notion of ∗This work was originally presented at the 16th International Symposium on Robotics Research, ISRR 2013. This revised version includes an extended description of the FMT∗ algorithm, proofs of all results, extended discussions about convergence rate and computational complexity, and a larger set of numerical experiments. 1 ar X iv :1 30 6. 35 32 v3 [ cs .R O ] 3 0 A pr 2 01 4 convergence in probability: the extra mathematical flexibility of this approach allows for convergence rate bounds – a first in the field of optimal sampling-based motion planning. Numerical experiments over a range of dimensions and obstacle configurations confirm our theoretical and heuristic arguments by showing that FMT∗ , for a given execution time, returns substantially better solutions than either PRM∗ or RRT∗ , especially in high-dimensional configuration spaces and in scenarios where collision checking is expensive.